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The  Most  Accurate  Path  from  Point  A  to  Point  B  is  Not 
Necessarily  a  Straight  Line 

Adam  J.  Rutkowski 

Air  Force  Research  Laboratory,  Eglin  AFB,  FL,  32542,  USA 


Abstract 

This  work  studies  the  problem  of  guiding  a  vehicle  from  a  known  initial  location  to  a  known  goal 
location  as  accurately  as  possible,  without  direct  observation  of  the  goal  location  (such  as  a  bearing 
measurement,  or  line-of-sight  to  the  goal),  and  without  direct  position  measurements,  such  as  those 
provided  by  GPS.  The  vehicle  travels  in  a  planar  environment  and  has  an  onboard  inertial  measurement 
unit  and  an  onboard  visual  system  to  measure  bearing  angles  to  features  in  the  environment.  Taking 
a  zigzagging  path  toward  the  goal  provides  better  position  estimation  than  a  straight  path.  For  a 
given  energy  budget,  there  is  a  certain  path  width,  or  amplitude,  that  results  in  the  best  estimation 
performance,  and  this  optimal  path  width  depends  on  the  sensor  noise  parameters. 

A  batch  estimator  is  derived  to  analyze  the  effect  of  the  entire  time  history  of  the  vehicle  trajectory 
on  final  position  estimation  performance.  The  formulation  results  in  a  linear  system  of  equations.  The 
path  width  that  minimizes  the  condition  number  of  the  system  matrix  also  minimizes  the  final  position 
estimation  error  when  the  feature  bearing  measurement  noise  is  relatively  large  compared  to  the  inertial 
measurement  noise. 


Nomenclature 

a  acceleration 

b  accelerometer  bias 

Cd  drag  coefficient 

J  estimation  objective  function 

x  position  vector,  [x  y]T 

F  vehicle  thrust,  N 

to  vehicle  mass,  kg 

At  timestep  length,  s 

/3  inverse  correlation  time,  1/s 

r]  random  disturbance 

9  bearing,  degrees 

Superscripts 
i  timestep 

k  feature  number 

Subscripts 
f  feature 

g  goal 

v  vehicle 

Accent  symbols 

estimated  quantity 
measured  quantity 


This  paper  appeared  in  the  Proceedings  of  the  2012  AIAA  Guidance,  Navigation,  and  Control  Conference  as  paper  number 
AIAA  2012-4761 
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1  Introduction 


The  objective  of  this  work  is  to  guide  a  vehicle  from  a  known  initial  location  to  a  known  goal  location  as 
accurately  as  possible,  without  direct  observation  of  the  goal  location  (such  as  a  bearing  measurement,  or 
line-of-sight  to  the  goal),  and  without  relying  on  a  direct  position  measurement  system  such  as  the  Global 
Positioning  System  (GPS).  Several  researchers  have  demonstrated  that  a  visual  system  is  an  effective  aid  to 
an  inertial  navigation  system  when  GPS  is  not  available  [4,  5].  Furthermore,  it  has  been  demonstrated  that 
vehicle  maneuvers  can  be  used  to  improve  position  estimation  accuracy.  Bryson  and  Sukkarielr  showed  that 
flying  an  occasional  orbit  maneuver  or  s-turn  improves  both  heading  and  vehicle  position  estimation  accuracy 
[1] .  Brink  et  al.  demonstrated  that  estimation  accuracy  can  be  improved  when  a  vehicle  takes  a  continuously 
zigzagging  path  toward  a  goal  [2].  In  general,  estimation  accuracy  improves  as  both  the  frequency  and 
amplitude  of  the  zigzagging  path  increase.  However,  paths  with  higher  frequency  and  amplitude  require 
more  energy,  and  without  a  kinetic  model  of  the  vehicle  (taking  into  consideration  mass,  drag,  etc.),  it  is 
not  possible  to  determine  which  combination  of  frequency  and  amplitude  yield  the  best  results  for  a  given 
energy  budget.  Furthermore,  the  work  of  Brink  et  al.  did  not  include  a  guidance  law  to  direct  the  vehicle 
to  the  goal;  rather,  the  vehicle  made  general  progress  along  one  Cartesian  axis  toward  the  goal. 

Frew  et.  al  have  examined  the  problem  of  traveling  from  a  known  start  location  to  a  known  goal  location 
without  position  measurements  [3].  They  developed  a  control  strategy  based  on  minimizing  an  objective 
function  that  is  the  sum  of  four  components  -  a  control  cost  that  limits  control  input,  a  navigation  cost 
that  moves  the  vehicle  toward  the  goal,  an  uncertainty  cost  that  moves  the  vehicle  to  improve  knowledge  of 
feature  locations,  and  a  safety  cost  that  is  used  to  avoid  obstacles.  However,  they  did  not  attempt  to  reach 
the  goal  as  accurately  as  possible;  rather,  they  attempted  to  reach  the  goal  to  within  a  specified  distance, 
then  return  to  the  start  location. 

In  this  work,  a  guidance  law  is  developed  to  maneuver  a  vehicle  in  a  continuously  zigzagging  manner 
such  that  a  constraint  on  the  amount  of  energy  (or,  more  accurately,  impulse)  expended  by  the  vehicle  is 
obeyed  using  a  constant  magnitude  force  over  a  specified  time.  In  the  final  stage  of  the  trajectory,  a  terminal 
guidance  algorithm  directs  the  vehicle  to  the  goal.  A  batch  estimator  is  also  developed  to  examine  the  effect 
of  the  entire  trajectory  history  on  final  position  estimation  accuracy. 


2  Approach 

For  simplicity,  the  problem  of  traveling  from  a  known  initial  location  to  a  goal  location  without  position 
feedback  is  considered  in  2D,  as  shown  in  Figure  1.  The  vehicle  has  an  onboard  IMU  and  vision  system. 
The  IMU  measures  vehicle  acceleration  along  the  x  and  y  axes,  while  the  vision  system  measures  the  bearing 
angle  to  prominent  visual  features  (e.g.  SIFT  features  or  Harris  corners)  in  the  environment.  The  vehicle 
orientation  is  constant  and  aligned  with  the  global  coordinate  frame.  The  features  are  at  initially  unknown 
locations  and  are  observed  ”on-the-fly” .  Since  the  position  of  the  vehicle  and  the  features  must  be  determined, 
this  is  a  simultaneous  localization  and  mapping  (SLAM)  problem  with  the  added  requirement  that  the  vehicle 
must  reach  a  goal  location  as  accurately  as  possible. 

2.1  Kinetics 

The  vehicle  kinetics  are  modeled  using  the  decoupled  set  of  equations  in  (1),  where  the  drag  force  components, 
Dx  and  Dy,  are  proportional  to  the  velocity  components  along  their  respective  axes. 

TTIXy  —  Fx  Gr/.i'v;  / ,  , 

myv  =  Fy~  Cdyv  { 

The  solution  of  the  kinetics  equation  along  the  x-axis  is  given  by  (2),  and  the  solution  along  the  y-axis 
has  the  same  form. 
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Figure  1:  Problem  scenario. 
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2.2  Sensing 

The  IMU  model  provides  an  acceleration  measurement,  a,  that  is  the  sum  of  the  true  acceleration,  x,  a 
Gaussian  white  noise  component,  rja ,  and  a  bias  component,  b,  as  given  by  (3). 


al=xi  +  V+ri  (3) 

The  bias  follows  a  first  order  Gauss-Markov  process  described  by  (4)  and  (5),  where  the  Gaussian  white 
noise  term  rj  Ab  has  a  standard  deviation  of  a  a  b,  and  over  time,  the  bias  will  have  a  standard  deviation  of  at,- 

<JAb  =  (1  -  exp(-2/3aAf))°'5<76  (4) 


bl  =  exp(— /3aAf)6*  1  +  rfAb  (5) 

The  visual  system  model  is  omnidirectional  and  capable  of  identifying  features  at  an  infinite  distance. 
The  feature  bearing  measurement  is  computed  according  to  (6),  where  atan (y,x)  is  the  4  quadrant  inverse 
tangent,  and  the  bearing  measurement  noise,  r/g ,  is  zero-mean  Gaussian  with  a  standard  deviation  ag. 

6  =  atan(j//  -  yv,xf  -  xv)  +r]g  (6) 


2.3  Estimation 


Since  the  estimation  performance  depends  on  the  entire  trajectory  history,  a  batch  estimator  is  used  to  solve 
for  all  states  at  all  points  in  time.  A  batch  estimator  was  chosen  instead  of  a  recursive  estimator,  such 
as  an  Extended  Kalman  Filter  (EKF),  with  the  hope  to  gain  insight  into  how  the  entire  time  history  of 
the  trajectory  influences  the  estimation  performance.  Furthermore,  the  batch  estimator  does  not  require  a 
delayed  feature  initialization  scheme  that  is  commonly  used  with  an  EKF  [1], 

The  state  vector  is  arranged  according  to  (7),  where  x/  =  [. x ^  yj  ...  xjf  yJf]T,  b  =  [bx  by]T,  and 
x„  =  [x\  yl  ...  x y  yy]T  for  N  total  timesteps  and  Nf  total  features. 


x  =  x, 


-TiT 


(7) 
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Notice  that  although  the  accelerometer  bias  is  modeled  as  a  first  order  Gauss-Markov  process  (as  in  (5)), 
the  accelerometer  bias  is  treated  as  constant  in  the  state  vector.  This  is  done  to  simplify  the  estimation 
procedure  and  reduce  the  dimension  of  the  resulting  equations. 

The  state  vector  is  estimated  by  minimizing  an  estimation  objective  function  (8)  that  consists  of  two 
parts,  a  feature  bearing  estimation  error,  Jf,  defined  in  (9),  and  an  acceleration  error,  Ja,  defined  in  (10). 


n  nf  n— 1 


j  =  wi’kJf(^  k)  +  xJ2 

2—1  k—  1  2—2 

(8) 

J fij'ii  ^0  — 

({K  -  X1})  sin^/fc  -  (vl  -  v))  cos^f) 

(9) 

Jaiti)  =  ( 

K  K  -  bl))  +  (yv  -  (a;  -  bl)) 

(10) 

The  feature  bearing  objective  function  of  (9)  is  derived  from  the  fact  that  in  the  ideal  case,  tan(d)  = 
sin(0)/cos(0)  =  ( yf  —  yv)/(xf  —  Xv).  At  a  given  moment  in  time,  multiplying  Jf  by  weighting  terms 
wl,k  and  summing  over  all  features  leads  to  an  objective  function  that  is  the  basis  of  the  classic  Stansfield 
estimator  [6].  If  the  weighting  terms,  w1,k,  are  chosen  appropriately,  the  Stansfield  estimator  provides  an 
elegant  approximation  to  the  maximum  likelihood  estimator  for  vehicle  position  when  the  feature  locations 
are  known.  However,  for  the  current  problem,  the  features  are  in  unknown  locations.  This  is  where  the 
acceleration  error  term  comes  in.  The  acceleration  objective  function  describes  the  disagreement  between 
the  acceleration  measurements  and  estimates,  and  the  term  A  describes  the  relative  importance  between 
acceleration  errors  and  bearing  errors. 

The  estimated  acceleration  is  derived  directly  from  the  estimated  position  using  the  central  difference 
scheme  of  (11). 


Ai+ 1  _  O Ai  I  Ai—  1 

J-'y _ V  ~  v 

(Aip 


(11) 


The  state  estimate  is  determined  by  setting  the  derivative  of  J  with  respect  to  x  to  zero.  This  leads  to 
the  following  linear  system,  where  the  system  matrix  L  depends  only  on  the  feature  bearing  measurements, 
and  the  vector  R  depends  only  on  the  accelerometer  measurements. 


Li  =  R  (12) 

The  equation  (12)  is  solved  twice,  once  with  the  weights  wl,k  set  to  one  for  all  features  at  all  points  in 
time,  then  again  with  the  weights  proportional  to  the  inverse  of  the  square  of  the  estimated  range  from  the 
first  solution.  The  improvement  in  estimation  accuracy  that  results  by  adjusting  the  weights  in  this  manner 
and  solving  (12)  a  second  time  will  be  demonstrated  in  the  results  section. 

In  the  matrix  L,  there  are  several  terms  involving  sin2(0),  cos2(0),  and  sin(0)  cos(0).  If  these  terms  are 
computed  directly  from  the  bearing  measurements,  the  noise  in  9  will  cause  these  terms  to  be  biased  from 
their  expected  values.  This  will  in  turn  cause  the  vehicle  and  feature  position  estimates  to  be  biased,  as  will 
be  shown  in  the  results  section.  This  can  be  fixed  if  one  knows  the  magnitude  of  the  noise  in  9,  which  can 
be  determined  from  a  calibration  procedure  of  the  bearings  measurement  device.  The  corrections  in  (13)  are 
based  on  approximating  sin (rjg)  «  rje  and  cos(rje)  «1-  r/g. 


2/m  _  (!  -  00 )  sin2(6>)  -  <jg  cos 2{0) 


sin  (0) 


cos 2 (9) 


1  -  2  a\ 


(1  —  Oq)  cos 2 (9)  —  Gq  sm2(0) 


1  -  2a g 


sin(0)  cos (9) 


sin(0)  cos (9) 
1  -  2 a2 


(13) 


4 

DISTRIBUTION  A 


2.4  Guidance 

2.4.1  Midcourse 

A  natural  way  to  achieve  the  excitation  necessary  for  observability  of  the  states  is  to  simply  let  the  vehicle 
zigzag  toward  the  goal.  To  achieve  this  behavior,  the  vehicle  exerts  a  constant  thrust  along  the  x-axis,  Fx, 
and  switches  the  sign  of  the  constant  magnitude  lateral  force,  Fy,  whenever  the  vehicle  moves  from  a  location 
between  two  parallel  boundaries  to  a  location  outside  the  boundaries.  These  boundaries  are  labeled  as  ijlb 
and  yUB  in  Figure  1,  where  yUB  =  Vbo  +  Vb  and  ulb  =  Ubo  -  Vb- 

2.4.2  Terminal 

After  a  certain  amount  of  time,  the  vehicle  is  directed  to  a  prespecified  goal  location  using  Algorithm  1. 
For  this  work,  only  the  y-coordinate  of  the  goal  location,  yg ,  is  specified.  At  a  given  time  tl  <  tN ,  y*  is 
the  position  along  the  y-axis  that  the  vehicle  would  reach  at  time  tN  if  Fy  =  —Fymag  sign(y  —  yg),  and  the 
time-to-go  tgo  =  tN  —  tl.  During  midcourse,  when  y*  gets  close  to  yg ,  this  is  the  point  when  Fy  must  point 
toward  the  goal,  otherwise,  the  vehicle  would  never  reach  the  goal.  The  points  y**  and  y***  are  also  checked 
when  the  condition  specified  by  y*  alone  does  not  allow  sufficient  time  to  reach  the  goal.  Expressions  for  y* , 
y**  and  y***  are  given  in  (14)-(16). 


Algorithm  1  Terminal  Guidance 
if  (|y*|  >  0.9%*)  then 
Fy  =  -Fymag  sign(y ) 

else  if  ( y **  >  yg  and  y‘v  <  yg  and  yl  >  0)  then 

T?i  _  _ Z7> 

J  y  ~  r  ymag 

else  if  (y***  <  yg  and  ylv  >  yg  and  y 1  <  0)  then 

zpi  _  zp 

J  y  ~  r  ymag 

end  if 


y*  =  %9o  +  (  -V'  sign (yl)  -  )  7^  (  1  -  exp  (  - 
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(14) 

(15) 

(16) 


The  meaning  of  y**  and  y***  is  apparent  in  Figure  2.  Near  the  point  xv  =  70,  with  no  terminal  guidance, 
Fy  is  positive  and  remains  so  until  the  vehicle  reaches  the  upper  bound  yuB  =  3.  However,  the  vehicle 
does  not  reach  the  goal.  Using  only  y*  guidance,  the  sign  of  Fy  switches  to  negative  at  the  point  when  yv 
becomes  positive,  but  this  thrust  cannot  overcome  the  momentum  of  the  vehicle  to  force  it  back  to  the  goal 
location  in  time.  Using  the  full  terminal  guidance  law,  y**  becomes  >  yg  when  yv  is  still  <  yg,  thus  from 
Algorithm  1,  the  sign  of  Fy  switches  to  negative  just  in  time  for  the  vehicle  to  reach  yg  at  tN . 


3  Results 

In  the  first  test  scenario,  there  are  two  rows  of  feature  points,  one  row  of  feature  points  located  at  y  =  15 
and  the  other  at  y  =  —15.  The  features  are  equally  spaced  along  each  row  with  a  spacing  of  15  m.  The 
vehicle  travels  for  150  seconds,  and  the  bearings  sensor  and  inertial  sensor  take  measurements  at  10  Hz. 

The  resulting  trajectory  estimate  when  ys  =  5  is  shown  in  Figure  3.  The  mean  final  position  estimation 
error  is  0.16  nr  over  1000  Monte  Carlo  runs.  In  comparison,  the  dead-reckoned  final  position  estimate  using 
only  IMU  data  sampled  at  100  Hz  with  perfect  bias  initialization  had  a  mean  error  of  33.85  m  as  computed 
from  1000  Monte  Carlo  runs. 
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Figure  2:  (left)  Effect  of  terminal  guidance  on  trajectory.  The  goal  location  has  yg  =  0.  (right)  Applied 
lateral  force. 


For  one  of  the  Monte  Carlo  runs,  the  true  and  estimated  accelerometer  bias  are  also  plotted  in  Figure  3. 
The  estimated  components  (4.93xl0-3,  1.588xl0-2)  of  the  accelerometer  bias  are  nearly  equal  to  the  mean 
of  the  true  bias  components  (5.29x  10— 3 ,  1.595x  10-2),  which  is  the  best  one  can  hope  for  when  approximating 
a  varying  quantity  as  a  constant. 

It  is  clear  from  Figure  3  that  the  corrections  given  in  (13)  are  necessary  to  avoid  a  biased  final  position 
estimate.  Furthermore,  solving  (12)  twice,  where  the  weights  for  the  second  solution  are  based  on  the  first 
solution,  produces  a  lower  mean  final  position  error  than  the  first  solution  (0.16  m  vs.  0.23  nr).  The  weights 
could  be  adjusted  again  based  on  the  second  solution  of  (12),  then  (12)  could  be  solved  a  third  time  with 
the  hope  of  obtaining  an  even  more  accurate  solution.  However,  for  this  particular  scenario,  no  improvement 
was  obtained  by  solving  (12)  a  third  time. 

The  mean  final  position  error  for  different  values  of  ys  and  two  different  values  for  the  standard  deviation 
of  the  bearings  sensor  measurement  noise  is  shown  in  Figure  3.  In  general,  it  appears  that  there  is  an  optimal 
spacing  between  the  bounds  that  results  in  the  best  estimation  performance,  and  this  optimal  spacing  tends 
to  be  lower  for  lower  values  of  erg.  The  exact  and  approximate  values  of  the  condition  number,  computed 
using  cond  and  condest  in  Matlab,  are  also  shown  in  Figure  3.  Notice  that  the  condition  number  and  the 
mean  final  position  error  are  both  minimized  at  nearly  the  same  value  of  t/b  when  erg  =  1°.  However,  when 
<jg  =  .3°,  the  noise  in  the  bearing  measurements  becomes  less  important  compared  to  the  noise  in  the  IMU 
measurements.  Since  the  vector  R  depends  on  the  IMU  measurements,  and  the  condition  number  indicates 
the  sensitivity  of  the  solution  of  (12)  to  changes  only  in  L  (and  thus  noise  in  the  bearing  measurements),  the 
condition  number  of  L  is  not  a  good  indicator  of  system  performance  when  the  bearing  measurement  noise 
is  relatively  small  compared  to  the  inertial  measurement  noise. 

The  batch  estimation  procedure  may  seem  like  a  computationally  expensive  approach,  but  the  linear 
system  given  in  (12)  can  actually  be  solved  quickly  due  to  the  sparsity  of  L.  For  this  particular  scenario, 
L  is  a  3024x3024  sparse  matrix,  and  the  solution  of  (12)  is  obtained  in  0.012  seconds  in  Matlab®on  an 
Intel®Xeon®W3550  CPU.  Recall  that  (12)  is  solved  twice  to  obtain  the  estimate,  first  with  the  weights  wl’k 
set  to  one,  then  again  with  the  weights  proportional  to  the  inverse  of  the  square  of  the  estimated  feature 
ranges.  When  the  overhead  required  to  form  L  and  R  is  taken  into  account,  each  trajectory  solution  in  our 
implementation  requires  about  0.05  seconds  to  compute  for  this  scenario. 

In  the  second  test  scenario,  there  is  only  one  row  of  feature  points  located  at  y  =  15.  Figure  4  shows  the 
mean  estimation  error  for  different  values  of  ys  and  yBO-  The  trajectory  is  also  shown  when  ys  =  5  and 
yBO  =  15-  Notice  that  when  yso  is  closer  to  the  feature  points,  the  bounds  do  not  need  to  be  as  far  apart 
to  achieve  a  desirable  estimation  accuracy.  Also,  in  contrast  to  the  previous  scenario,  the  estimation  error 
does  not  tend  to  increase  as  quickly  with  increasing  bound  separation. 
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Figure  3:  (top  left)  The  true  vehicle  trajectory  is  shown  for  ys  =  5  and  erg  =  1°,  along  with  an  estimate 
of  the  trajectory  from  one  of  the  1000  Monte  Carlo  runs  and  a  20er  covariance  ellipse  on  the  final  position 
estimate  for  all  Monte  Carlo  runs.  The  mean  estimation  error  in  final  position  in  this  case  is  0.16  m.  The  true 
feature  locations  are  marked  with  ’+’  symbols,  and  the  20er  covariance  ellipses  for  the  feature  locations  are 
also  shown.  The  bounds  yuB  and  j/bb  are  marked  by  parallel  lines,  (top  right)  The  true  accelerometer  bias 
and  its  estimate  are  plotted  over  time,  (middle  left)  Trajectory  estimation  results  with  ys  =  5  and  ag  =  1° 
without  applying  the  corrections  of  (13),  the  covariance  is  roughly  the  same,  but  the  final  position  estimate 
is  biased  by  -6.67  nr  along  the  x-axis.  (middle  right)  If  the  estimator  is  run  only  once  without  re-weighting 
based  on  estimated  feature  distance,  the  mean  estimation  error  in  final  position  increases  to  0.23  nr.  (bottom 
left)  The  mean  position  error,  computed  from  1000  Monte  Carlo  simulations  for  1  <  j/b  <  15  is  shown  for 
two  different  levels  of  feature  bearing  noise,  (bottom  right)  The  exact  and  approximate  condition  numbers 
of  the  matrix  L  as  respectively  computed  with  cond  and  condest  in  Matlab®are  shown  for  various  values  of 
Vs- 
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Figure  4:  (left)  The  mean  position  error  is  shown  for  various  values  of  ys  and  yso  in  the  second  test  scenario 
(described  in  the  text),  (right)  The  trajectory  when  ys  =  5  and  yso  =  15 


4  Conclusions  and  Future  Work 

In  this  work,  a  batch  estimator  for  vision-aided  inertial  navigation  was  developed  and  used  to  study  the 
problem  of  traveling  from  a  known  location  to  a  goal  location  without  position  feedback.  A  guidance  law 
was  developed  to  excite  the  motion  of  the  vehicle  to  improve  observability  of  the  vehicle  states.  For  a  given 
energy  budget,  there  is  a  certain  path  width,  or  amplitude,  that  results  in  the  best  estimation  performance, 
and  this  optimal  path  width,  as  defined  by  the  parameter  ys,  depends  on  the  sensor  noise  parameters.  The 
formulation  of  the  batch  estimator  results  in  a  linear  system  of  equations.  The  path  width  that  minimizes  the 
condition  number  of  the  system  matrix  also  minimizes  the  final  position  estimation  error  when  the  feature 
bearing  measurement  noise  is  relatively  large  compared  to  the  inertial  measurement  noise.  However,  if  the 
feature  bearing  measurement  noise  is  relatively  small,  the  condition  number  of  the  system  matrix  is  not  a 
good  indicator  of  the  final  position  estimation  error. 

Although  the  batch  estimator  developed  in  this  work  can  be  used  as  an  analysis  tool  for  determining  the 
optimal  path  width  that  results  in  minimum  final  position  error,  it  does  not  yet  help  in  developing  heuristic 
rules  for  determining  an  optimal  flight  path  while  in  flight.  It  is  only  useful  in  finding  the  optimal  path 
width  given  all  the  measurements  up  to  the  goal  location,  yet  the  measurements  of  course  are  not  known  a 
priori.  For  future  work,  the  condition  number  of  the  batch  estimator  may  be  used  to  discover  paths  that 
produce  even  better  estimation  performance  than  the  class  of  trajectories  explored  in  this  work.  Analysis  of 
the  information  provided  by  the  measurements  during  these  trajectories  may  then  be  used  to  develop  rules 
for  trajectory  planning  that  can  be  implemented  in  real  time. 
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